//
// Argon v0.1 - March 2009
// The Arduino Quadrocopter
// Ville Vilpas (Psoden)
//
#include <ServoTimer2.h>
#include <PID_Beta6.h>

// Allow << streaming
#include "WProgram.h"
void setup();
void loop();
void establishContact();
void sendLong(long data);
long receiveLong();
template<class T> inline Print &operator <<(Print &obj, T arg) { 
  obj.print(arg); 
  return obj; 
}

//*********** Initialize variables ***********//
#define DEBUG 0

// XBee
#define BAUD 115200
int inByte;
boolean firstContact = false;
union {
  byte b[4];
  long asLong;
} 
outBuffer; 
union {
  byte b[4];
  long asLong;
} 
inBuffer; 
// DEBUG
long buffer;

// Control
long txCommands[4];
#define ROLL 0
#define PITCH 1
#define YAW 2
#define THROTTLE 3

// Timing
int loopRate, previousRate, currentRate;
//************* Setup *************//
void setup() {
  Serial.begin(BAUD);
  //analogReference(EXTERNAL); // 3.3v
  establishContact();
}

//*********** Main loop ***********//
void loop() {
  if(Serial.available() > 16) {
    inByte = Serial.read();
    if(inByte == int('C')) {
      for(int k = 0; k < 4; k++) {
        txCommands[k] = receiveLong();
      }
      if(DEBUG) {
        if(txCommands[ROLL] == 123456) { 
          sendLong(1337); 
        } else { 
          sendLong(txCommands[ROLL]); 
        }
      } else {
        Serial.print(loopRate, BYTE);
      }
      Serial.flush();
    }
  }
  
  // Timing
  currentRate = millis();
  loopRate = currentRate - previousRate;
  if(loopRate > 250) { loopRate = 250; }
  previousRate = currentRate;
}

//
// Communication
//

void establishContact() {
  while (Serial.available() <= 0) {
    Serial.print('A', BYTE);
    delay(300);
  }
}

//************ Transmit functions ************//
void sendLong(long data) {
  outBuffer.asLong = data;
  Serial.print(outBuffer.b[0], BYTE);
  Serial.print(outBuffer.b[1], BYTE);
  Serial.print(outBuffer.b[2], BYTE);
  Serial.print(outBuffer.b[3], BYTE);
}

//************ Receive functions ************//
long receiveLong() {
  for(int i = 3; i >= 0; i--) {
    inByte = Serial.read();
    inBuffer.b[i] = byte(inByte);
  }
  return inBuffer.asLong;
}

int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}

